#include "ros/ros.h"
#include "geometry_msgs/Pose.h"
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include "biped_robin_msgs/PickupObjectService.h"
#include "biped_robin_msgs/PlaceObjectService.h"
#include "biped_robin_msgs/MovePostureCartesianService.h"
#include "biped_robin_msgs/MoveGripperService.h"
#include "std_srvs/Empty.h"
#include "sensorhand_speed/hand_cmd.h"
#include <string>

ros::ServiceClient movePostureCartesian_client;
ros::ServiceClient hand_client;
static tf::TransformListener* pListener = NULL;
static double arm_radius = 0;
static double arm_angle_0 = 0;
static double torso_radius = 0.46; //von Baselink zu Armgelenk
static double PI = 3.14159265359;
static double right_arm_angle_0 = 28; //Winkel für startPosture in Grad
static tf::Vector3 start_tcp_origin;
static tf::StampedTransform odom_base_link_transform;
static tf::StampedTransform right_arm_odom_transform;
static tf::StampedTransform right_arm_tcp_transform;

bool moveToGrasp(tf::StampedTransform object_transform, double offset, double z_offset);
bool moveHand(int open_close);
bool startPosture();

//Funktion zu anfahren einer Position
bool moveToGrasp(tf::StampedTransform object_transform, double offset, double z_offset){	

	biped_robin_msgs::MovePostureCartesianService srv;	

	tf::StampedTransform tcp_transform;	
	tf::StampedTransform right_arm_object_transform;

	//Aktuelle Position des TCP einlesen zum Berechnen der Weglaenge
	try {
			pListener->lookupTransform("/odom", "/right_tcp", ros::Time(0), tcp_transform);
		} catch (tf::TransformException ex) {
			ROS_INFO("reference_frame_id not found");
			return false;
	}	
	
	geometry_msgs::Quaternion upper_body_posture;
	geometry_msgs::Point right_tcp_position;
	tf::Quaternion posture;	

	double left_arm_angle = 0;
	double right_arm_angle = 0;
	double time_to_move = 1.5;
	double roll=0;		
	
	tf::Vector3 object_origin = object_transform.getOrigin();	
	right_arm_object_transform.mult(right_arm_odom_transform, object_transform);	
	tf::Vector3 right_arm_object = right_arm_object_transform.getOrigin();
	tf::Vector3 base_link_origin = odom_base_link_transform.getOrigin();

	//radius von baselink zu objekt	
	double r = sqrt((object_origin.getX()-base_link_origin.getX())*(object_origin.getX()-base_link_origin.getX())+(object_origin.getY()-base_link_origin.getY())*(object_origin.getY()-base_link_origin.getY()));
	//laenge von arm zum objekt mit baselink als drehgelenk
	double l = sqrt(r*r-0.22*0.22);
	double phi = atan2(l,0.22);
	double xsi = atan2(-(object_origin.getY()-base_link_origin.getY()),(object_origin.getX()-base_link_origin.getX()));	
	double yaw = (PI/2-xsi-phi)/1.5; //Oberkoerper drehung
	l -= offset;
	double pitch = l/3; //Oberkoerper Neigung
	right_arm_angle = -(2*pitch)-0.2-1.8*z_offset; //Armwinkel
	
	//pseudo objekt koordinaten mit offsets zum anfahren von pregrasp
	object_origin.setX(object_origin.getX()-offset*cos(yaw));
	object_origin.setY(object_origin.getY()-offset*sin(yaw));
	object_origin.setZ(object_origin.getZ()+z_offset);
		
	posture.setRPY(roll, pitch, yaw);

	//berechnen der weglaenge und der zeit in s für die bewegung
	tf::Vector3 object_tcp = object_origin-tcp_transform.getOrigin();
	time_to_move += object_tcp.length()*14;

	upper_body_posture.x = posture.getX();
	upper_body_posture.y = posture.getY();
	upper_body_posture.z = posture.getZ();
	upper_body_posture.w = posture.getW();	
	
	right_tcp_position.x = object_origin.getX();
	right_tcp_position.y = object_origin.getY();
	right_tcp_position.z = object_origin.getZ();

	srv.request.right_tcp_position = right_tcp_position;
	srv.request.upper_body_posture = upper_body_posture;
	srv.request.left_arm_angle = left_arm_angle;
	srv.request.right_arm_angle = right_arm_angle;
	srv.request.time_to_move = time_to_move;
	
	movePostureCartesian_client.call(srv);
	ros::Duration(time_to_move-1).sleep();
	return true;
}


//funktion zum oeffnen und schliessen der hand
bool moveHand(int open_close){
	sensorhand_speed::hand_cmd hand_srv;
	if(open_close == 1){
		hand_srv.request.cmd = 2;
		hand_srv.request.param = 0.5;
	} else {
		hand_srv.request.cmd = 3;
		hand_srv.request.param = 0.5;
	}		
	hand_client.call(hand_srv);
	return true;
}


//anfahren der start position
bool startPosture(){
	biped_robin_msgs::MovePostureCartesianService srv;
	sensorhand_speed::hand_cmd hand_srv;

	tf::StampedTransform tcp_transform;		

	try {
			pListener->lookupTransform("/odom", "/right_tcp", ros::Time(0), tcp_transform);
		} catch (tf::TransformException ex) {
			ROS_INFO("reference_frame_id not found");
			return false;
	}		
	
	geometry_msgs::Quaternion upper_body_posture;
	geometry_msgs::Point right_tcp_position;
	tf::Quaternion posture;	
	tf::Vector3 tcp_origin = start_tcp_origin;	
	
	double left_arm_angle = 0;
	double right_arm_angle = -right_arm_angle_0/180.0*PI;
	double time_to_move = 2;		
	
	//berechnen der tcp koordinaten ohne baselink zu verschieben
	double x_0 = arm_radius*sin(arm_angle_0);
	double z_0 = -arm_radius*cos(arm_angle_0);
	double x = arm_radius*sin(-right_arm_angle+arm_angle_0)-x_0;
	double z = -arm_radius*cos(-right_arm_angle+arm_angle_0)-z_0;
	posture.setRPY(0, 0, 0);

	upper_body_posture.x = posture.getX();
	upper_body_posture.y = posture.getY();
	upper_body_posture.z = posture.getZ();
	upper_body_posture.w = posture.getW();

	right_tcp_position.x = tcp_origin.getX()+x;
	right_tcp_position.y = tcp_origin.getY();
	right_tcp_position.z = tcp_origin.getZ()+z;
	
	tf::Vector3 object_tcp = tcp_origin-tcp_transform.getOrigin();
	time_to_move += object_tcp.length()*16;

	srv.request.right_tcp_position = right_tcp_position;
	srv.request.upper_body_posture = upper_body_posture;
	srv.request.left_arm_angle = left_arm_angle;
	srv.request.right_arm_angle = right_arm_angle;
	srv.request.time_to_move = time_to_move;

	movePostureCartesian_client.call(srv);
	ros::Duration(time_to_move-1).sleep();

	hand_srv.request.cmd = 7;
	hand_srv.request.param = 0;
	hand_client.call(hand_srv);
	moveHand(1);
	return true;
}

//callback zum aufnehmen eines Bechers
bool pickupCallback(biped_robin_msgs::PickupObjectService::Request &req, biped_robin_msgs::PickupObjectService::Response &res) {
	std::string object_frame_id = req.object.reference_frame_id;
	tf::StampedTransform object_transform;	
	try {
		pListener->lookupTransform("/odom", object_frame_id, ros::Time(0), object_transform);
	} catch (tf::TransformException ex) {
		ROS_INFO("reference_frame_id not found");
		return false;
	}	
	tf::Vector3 object_origin = object_transform.getOrigin();	
	//offsets aufgrund ungenauer tcp vermessung
	double xoffset = 0.14;
	double yoffset = 0.08;
	double zoffset = 0.06;
	object_origin.setX(object_origin.getX()+xoffset);	
	object_origin.setY(object_origin.getY()+yoffset);
	object_origin.setZ(object_origin.getZ()+zoffset);
	object_transform.setOrigin(object_origin);	
	moveHand(1);
	moveToGrasp(object_transform, 0.02, 0.06); //pregrasp
	moveToGrasp(object_transform, 0, 0.0); //grasp
	moveHand(0);
	ros::Duration(0.5).sleep();
	moveToGrasp(object_transform, -0.03, 0.14); //postgrasp
	return true;
}

//callback zum entsorgen des bechers
bool placeCallback(biped_robin_msgs::PickupObjectService::Request &req, biped_robin_msgs::PickupObjectService::Response &res) {
	std::string object_frame_id = req.object.reference_frame_id;
	tf::StampedTransform object_transform;	
	try {
		pListener->lookupTransform("/odom", object_frame_id, ros::Time(0), object_transform);
	} catch (tf::TransformException ex) {
		ROS_INFO("reference_frame_id not found");
		return false;
	}
	moveToGrasp(object_transform, 0, 0.10);
	moveHand(1);
	startPosture();
	ros::Duration(0.5).sleep();
	return true;
}

//callback zum anfahren der start postition
bool startPostureCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {	
	ROS_INFO("startPosture_srv called");
	return startPosture();	
}

int main(int argc, char **argv)
{
  /* init ROS */    
  ros::init(argc, argv, "biped_localizer");
	pListener = new(tf::TransformListener);
	ros::NodeHandle n;

	ros::Duration(1).sleep();

	//start transformationen einlesen um baselink nicht zu verschieben
	try {
			pListener->lookupTransform("/odom", "/base_link", ros::Time(0), odom_base_link_transform);
		} catch (tf::TransformException ex) {
			ROS_INFO("reference_frame_id not found");
			return false;
	}	

	try {
			pListener->lookupTransform("/right_arm", "/odom", ros::Time(0), right_arm_odom_transform);
		} catch (tf::TransformException ex) {
			ROS_INFO("reference_frame_id not found");
			return false;
	}	

	try {
			pListener->lookupTransform("/right_arm", "/right_tcp", ros::Time(0), right_arm_tcp_transform);
		} catch (tf::TransformException ex) {
			ROS_INFO("reference_frame_id not found");
			return false;
	}	
	
	//werte fuer den arm berechnens
	tf::Vector3 right_arm_tcp = right_arm_tcp_transform.getOrigin();
	arm_radius = sqrt(right_arm_tcp.getX()*right_arm_tcp.getX()+right_arm_tcp.getZ()*right_arm_tcp.getZ());
	arm_angle_0 = atan(right_arm_tcp.getX()/-right_arm_tcp.getZ());

	//start position des tcp
	start_tcp_origin.setX(0.2625);
	start_tcp_origin.setY(-0.0545);
	start_tcp_origin.setZ(0.73666);

	//service clients
	movePostureCartesian_client = n.serviceClient<biped_robin_msgs::MovePostureCartesianService>("movePostureCartesian_srv");
	hand_client = n.serviceClient<sensorhand_speed::hand_cmd>("hand_node/hand_srv");

	startPosture();

	//service servers	
	ros::ServiceServer pickup_srv = n.advertiseService("pickup_srv", pickupCallback);
	ros::ServiceServer place_srv = n.advertiseService("place_srv", placeCallback);
	ros::ServiceServer start_posture_srv = n.advertiseService("startPosture_srv" , startPostureCallback);	
	
	ros::spin();

  return 0;
}
